home *** CD-ROM | disk | FTP | other *** search
/ BBS in a Box 3 / BBS in a box - Trilogy III.iso / Files / Prog / D-G / GemsII / viewcorr / viewcorr.c < prev    next >
Encoding:
C/C++ Source or Header  |  1992-06-16  |  9.0 KB  |  311 lines  |  [TEXT/MPS ]
  1. /* 
  2.  * viewcorr.c - Iterate the view parameters.
  3.  *
  4.  * 
  5.  * Author:    Rod G. Bogart
  6.  * Date:    Oct 15 1990
  7.  * Copyright (c) 1990, University of Michigan
  8.  * 
  9.  */
  10. #include <stdio.h>
  11. #include <math.h>
  12. #include "GraphicsGems.h"
  13. #include "matrix.h"
  14. #include "viewcorr.h"
  15.  
  16. iterate_view_parms( datapts, view_parms, num_iterations )
  17. ViewData *datapts;
  18. ViewParms *view_parms;
  19. int num_iterations;
  20. {
  21.     Matrix errors, jacobian, corrections;
  22.     int i,j;
  23.     double rootmeansqr, last_rootmeansqr;
  24.  
  25.     /* allocate Matrix stuff */
  26.     errors = NewMatrix(datapts->numpts*2, 1);
  27.     jacobian = NewMatrix(datapts->numpts*2, NUM_VIEW_PARMS);
  28.     corrections = NewMatrix(NUM_VIEW_PARMS, 1);
  29.  
  30.     if (num_iterations <= 0) {
  31.     num_iterations = 10000;
  32.     }
  33.     last_rootmeansqr = 0.0;
  34.     for (i = 0; i < num_iterations; i++)
  35.     {
  36.     measure_errors( datapts, view_parms, errors, &rootmeansqr );
  37.     if (ABS(rootmeansqr - last_rootmeansqr) < 1E-8) {
  38.         /* quit when rootmeansqr stays the same */
  39.         break;
  40.     }
  41.     last_rootmeansqr = rootmeansqr;
  42.     if (rootmeansqr > (0.1 * view_parms->halfx)) 
  43.         /* When the error terms are large, the corrections become too
  44.          * extreme, and knock the whole thing into outer space.  Sooo,
  45.          * shrink the error terms, to cause the corrections to happen
  46.          * a small amount at a time.  Note: dividing by the rootmeansqr
  47.          * may be a little extreme, but it does slow down the erratic
  48.          * behaviour.
  49.          */
  50.         for (j = 0; j < datapts->numpts; j++)
  51.         {
  52.         errors[0][j*2] /= rootmeansqr;
  53.         errors[0][j*2+1] /= rootmeansqr;
  54.         }
  55.     build_jacobian( datapts, view_parms, jacobian );
  56.     find_corrections( datapts, jacobian, errors, corrections );
  57.     apply_corrections( corrections, view_parms );
  58.     }
  59.     FreeMatrix(errors, 1);
  60.     FreeMatrix(jacobian, NUM_VIEW_PARMS);
  61.     FreeMatrix(corrections, 1);
  62. }
  63.  
  64. measure_errors( datapts, view_parms, errors, rootmeansqr )
  65. ViewData *datapts;
  66. Matrix errors;
  67. ViewParms *view_parms;
  68. double *rootmeansqr;
  69. {
  70.     int i;
  71.     double sqrs=0.0;
  72.     Point2 screenpt;
  73.  
  74.     for (i = 0; i < datapts->numpts; i++)
  75.     {
  76.     screen_project( &datapts->pts[i], view_parms, &screenpt );
  77.     errors[0][i*2 + 0] = screenpt.x - datapts->scrpts[i].x;
  78.     errors[0][i*2 + 1] = screenpt.y - datapts->scrpts[i].y;
  79.     sqrs += SQR(errors[0][i*2 + 0]) + SQR(errors[0][i*2 + 1]);
  80.     }
  81.     *rootmeansqr = sqrt( sqrs / (datapts->numpts*2.0) );
  82. }
  83.  
  84. build_jacobian( datapts, view_parms, jacobian )
  85. ViewData *datapts;
  86. Matrix jacobian;
  87. ViewParms *view_parms;
  88. {
  89.     int i;
  90.     Point3 xyz, eR;
  91.  
  92.     /* The jacobian matrix has at least 10 columns (u and v for 5 pts) 
  93.      * and 10 rows (10 iteration parameters).  The iteration parameters will
  94.      * be ordered: eRx eRy eRz phi_x phi_y phi_z ds xcenter ycenter aspect
  95.      * from the top down.
  96.      */
  97.     
  98.     V3MulPointByMatrix(&view_parms->eye, &view_parms->view, &eR);
  99.     for (i = 0; i < datapts->numpts; i++)
  100.     {
  101.     V3MulPointByMatrix(&datapts->pts[i], &view_parms->view, &xyz);
  102.  
  103.     store_u_partials( &xyz, &eR, view_parms, i, jacobian );
  104.     store_v_partials( &xyz, &eR, view_parms, i, jacobian );
  105.     }
  106. }
  107.  
  108. store_u_partials( xyz, eR, view_parms, ptnum, jacobian )
  109. Point3 *xyz, *eR;
  110. Matrix jacobian;
  111. ViewParms *view_parms;
  112. int ptnum;
  113. {
  114.     double x_min_eR, z_min_eR;
  115.     int i2;
  116.  
  117.     i2 = ptnum*2;
  118.     x_min_eR = xyz->x - eR->x;
  119.     z_min_eR = xyz->z - eR->z;
  120.  
  121.     jacobian[0][i2] = view_parms->d_over_s * view_parms->halfx / z_min_eR;
  122.     jacobian[1][i2] = 0.0;
  123.     jacobian[2][i2] = - view_parms->d_over_s * view_parms->halfx * x_min_eR
  124.     / SQR( z_min_eR );
  125.  
  126.     jacobian[3][i2] = view_parms->d_over_s * view_parms->halfx * xyz->y 
  127.     * x_min_eR / SQR( z_min_eR );
  128.     jacobian[4][i2] = - (view_parms->d_over_s * view_parms->halfx * xyz->z 
  129.              / z_min_eR) 
  130.     - (view_parms->d_over_s * view_parms->halfx * xyz->x * x_min_eR
  131.        / SQR( z_min_eR ));
  132.     jacobian[5][i2] = view_parms->d_over_s * view_parms->halfx * xyz->y 
  133.     / z_min_eR;
  134.  
  135.     jacobian[6][i2] = - view_parms->halfx * x_min_eR / z_min_eR;
  136.     jacobian[7][i2] = 1.0;
  137.     jacobian[8][i2] = 0.0;
  138. #ifdef ITERATE_ASPECT_RATIO
  139.     jacobian[9][i2] = 0.0;
  140. #endif
  141. }
  142.  
  143. store_v_partials( xyz, eR, view_parms, ptnum, jacobian )
  144. Point3 *xyz, *eR;
  145. Matrix jacobian;
  146. ViewParms *view_parms;
  147. int ptnum;
  148. {
  149.     double y_min_eR, z_min_eR, d_over_s;
  150.     int i2;
  151.  
  152.     i2 = ptnum*2 + 1;
  153.     y_min_eR = xyz->y - eR->y;
  154.     z_min_eR = xyz->z - eR->z;
  155.     d_over_s = view_parms->d_over_s * view_parms->aspect;
  156.  
  157.     jacobian[0][i2] = 0.0;
  158.     jacobian[1][i2] = d_over_s * view_parms->halfx / z_min_eR;
  159.     jacobian[2][i2] = - d_over_s * view_parms->halfx * y_min_eR
  160.     / SQR( z_min_eR );
  161.  
  162.     jacobian[3][i2] = (d_over_s * view_parms->halfx * xyz->z
  163.                / z_min_eR)
  164.     + (d_over_s * view_parms->halfx * xyz->y * y_min_eR
  165.        / SQR( z_min_eR ));
  166.     jacobian[4][i2] = - d_over_s * view_parms->halfx * xyz->x
  167.     * y_min_eR / SQR( z_min_eR );
  168.     jacobian[5][i2] = - d_over_s * view_parms->halfx * xyz->x
  169.     / z_min_eR;
  170.  
  171.     jacobian[6][i2] = - view_parms->aspect * view_parms->halfx * y_min_eR
  172.     / z_min_eR;
  173.     jacobian[7][i2] = 0.0;
  174.     jacobian[8][i2] = 1.0;
  175. #ifdef ITERATE_ASPECT_RATIO
  176.     jacobian[9][i2] = - view_parms->d_over_s * view_parms->halfx * y_min_eR
  177.     / z_min_eR;
  178. #endif
  179. }
  180.  
  181. find_corrections( datapts, jacobian, errors, corrections )
  182. ViewData *datapts;
  183. Matrix jacobian, errors, corrections;
  184. {
  185.     Matrix jacobian_transpose, combo_inverse, error_J_transpose;
  186.     int i;
  187.  
  188.     /* The corrections matrix is the error matrix times the inverse
  189.      * of the Jacobian.  Since the Jacobian may not be square, the
  190.      * pseudo-inverse is used:
  191.      *           -1        T      T -1
  192.      *   C = E  J   =  E  J  (J  J )
  193.      */
  194.     for (i = 0; i < NUM_VIEW_PARMS; i++)
  195.     corrections[0][i] = 0.0;
  196.  
  197.     jacobian_transpose = NewMatrix(NUM_VIEW_PARMS, datapts->numpts*2);
  198.     combo_inverse = NewMatrix(NUM_VIEW_PARMS, NUM_VIEW_PARMS);
  199.  
  200.     TransposeMatrix( jacobian, jacobian_transpose, 
  201.              NUM_VIEW_PARMS, datapts->numpts*2 );
  202.     MultMatrix( jacobian, jacobian_transpose, combo_inverse, 
  203.            NUM_VIEW_PARMS, datapts->numpts*2, NUM_VIEW_PARMS );
  204.     if (InvertMatrix( combo_inverse, NUM_VIEW_PARMS ) == 0.0)
  205.     {
  206.     fprintf(stderr,"Could not invert matrix in iteration!!!\n");
  207.     FreeMatrix(jacobian_transpose, datapts->numpts*2);
  208.     FreeMatrix(combo_inverse, NUM_VIEW_PARMS);
  209.     return;
  210.     }
  211.  
  212.     error_J_transpose = NewMatrix(NUM_VIEW_PARMS, 1);
  213.     MultMatrix( errors, jacobian_transpose, error_J_transpose, 1, 
  214.             datapts->numpts*2, NUM_VIEW_PARMS );
  215.     MultMatrix( error_J_transpose, combo_inverse, corrections, 1, 
  216.            NUM_VIEW_PARMS, NUM_VIEW_PARMS );
  217.     FreeMatrix(jacobian_transpose, datapts->numpts*2);
  218.     FreeMatrix(combo_inverse, NUM_VIEW_PARMS);
  219.     FreeMatrix(error_J_transpose, 1);
  220. }
  221.  
  222. apply_corrections( corrections, view_parms )
  223. Matrix corrections;
  224. ViewParms *view_parms;
  225. {
  226.     ViewParms current_parms;
  227.     Point3 eR;
  228.     Matrix3 inc_rotate;
  229.  
  230.     current_parms = *view_parms;
  231.  
  232.     build_rotate(&inc_rotate, 
  233.          -corrections[0][3], -corrections[0][4], -corrections[0][5]);
  234.     V2MatMul(¤t_parms.view, &inc_rotate, &view_parms->view);
  235.     propagate_rotate_change( view_parms );
  236.  
  237.     V3MulPointByMatrix(¤t_parms.eye, ¤t_parms.view, &eR);
  238.     eR.x -= corrections[0][0];
  239.     eR.y -= corrections[0][1];
  240.     eR.z -= corrections[0][2];
  241.     V3MulPointByMatrix(&eR, &view_parms->viewinv, &view_parms->eye);
  242.     view_parms->d_over_s -= corrections[0][6];
  243.     view_parms->xcenter -= corrections[0][7];
  244.     view_parms->ycenter -= corrections[0][8];
  245. #ifdef ITERATE_ASPECT_RATIO
  246.     view_parms->aspect -= corrections[0][9];
  247. #endif
  248.     return;
  249. }
  250.  
  251. propagate_rotate_change( view_parms )
  252. ViewParms *view_parms;
  253. {
  254.     /* inverse is just the transpose of a rotate matrix */
  255.     TransposeMatrix3(&view_parms->view, &view_parms->viewinv);
  256. }
  257.  
  258. rotate_mat(m, rot_angle, pos_sin_index, neg_sin_index)
  259. Matrix3 *m;
  260. double rot_angle;
  261. int pos_sin_index, neg_sin_index;
  262. {
  263.     double cos_theta, sin_theta;
  264.     int i,j;
  265.  
  266.     cos_theta = cos( rot_angle );    sin_theta = sin( rot_angle );
  267.     for (i = 0; i < 3; i++)
  268.     for (j = 0; j < 3; j++)
  269.         m->element[i][j] = (i==j) ? 1.0 : 0.0;
  270.     m->element[pos_sin_index][pos_sin_index] = cos_theta;
  271.     m->element[neg_sin_index][neg_sin_index] = cos_theta;
  272.     m->element[pos_sin_index][neg_sin_index] = sin_theta;
  273.     m->element[neg_sin_index][pos_sin_index] = -sin_theta;
  274. }
  275.  
  276. build_rotate( m, rot_x, rot_y, rot_z )
  277. Matrix3 *m;
  278. double rot_x, rot_y, rot_z;
  279. {
  280.     Matrix3 tmpmat, rotate;
  281.  
  282.     /* m = Xrotate
  283.      * tmpmat = [m] [Yrotate]
  284.      * m = [tmpmat] [Zrotate]
  285.      */
  286.  
  287.     rotate_mat(m, rot_x, 1, 2);
  288.     rotate_mat(&rotate, rot_y, 2, 0);
  289.     V2MatMul(m, &rotate, &tmpmat);
  290.     rotate_mat(&rotate, rot_z, 0, 1);
  291.     V2MatMul(&tmpmat, &rotate, m);
  292. }
  293.  
  294. screen_project(datapt, view_parms, screenpt)
  295. Point3 *datapt;
  296. ViewParms *view_parms;
  297. Point2 *screenpt;
  298. {
  299.     Point3 xyz, data_minus_eye;
  300.     int i;
  301.  
  302.     V3Sub(datapt, &view_parms->eye, &data_minus_eye);
  303.     V3MulPointByMatrix(&data_minus_eye, &view_parms->view, &xyz);
  304.     screenpt->x = - (view_parms->d_over_s * view_parms->halfx * xyz.x 
  305.              / xyz.z) + view_parms->xcenter;
  306.     screenpt->y = - (view_parms->aspect * view_parms->d_over_s
  307.              * view_parms->halfx * xyz.y 
  308.              / xyz.z) + view_parms->ycenter;
  309. }
  310.  
  311.